using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace Autobot
{
    class Britishgorithm : AlgorithmBase
    {
        // Static string to identify this class in the Algorithms class
        public static readonly new string Name = "Britishgorithm";

        int current_speed;
        float current_angle;
        //int state = 0;
        
        //O PATH VARS
        bool orig_set = false;
        float ox;
        float oy;
        float o_slope;
        float o_slope_inv;
        float o_angle;

        // Store a reference to the world objects
        public Britishgorithm(VirtualMap mapReference, Robot robotReference, Goal goalReference)
        {
            map = mapReference;
            robot = robotReference;
            goal = goalReference;
            current_speed = 0;
        }

        private float GetDistanceFromOPath(float rx, float ry)
        {
            if (orig_set)
            {
                float x = (o_slope * ox - o_slope_inv * rx + ry - oy) / (o_slope - o_slope_inv);
                float y = o_slope_inv * (x - rx) + ry;
                return (float)Math.Sqrt( (x - rx) * (x - rx) + (y - ry) * (y - ry) );
            }

            return 0;
        }

        private void SetOPath(float rx, float ry, float gx, float gy)
        {
            orig_set = true;
            ox = rx;
            oy = ry;
            o_slope = (ox - gx) / (oy - gy);
            o_slope_inv = (oy - gy) / (ox - gx);
            o_angle = (float)Math.Atan2(gx - rx, gy - ry);
        }

        public override AlgorithmResult GetNextResult()
        {
            AlgorithmResult result = new AlgorithmResult();

            
            



            result.AddCommand(CreateIncSpeedCommand(current_speed++, 0));
            // Lord British is invincible, therefore, he doesn't have to do anything but go faster


            float rx = robot.GetVirtualPosition().X;
            float ry = robot.GetVirtualPosition().Y;
            float gx = goal.GetVirtualPosition().X;
            float gy = goal.GetVirtualPosition().Y;

            if (!orig_set)
            {
                SetOPath(rx, ry, gx, ry);
            }

            float sensor_l = robot.GetSensorReading( (int)SensorID.LEFT );
            float sensor_m = robot.GetSensorReading( (int)SensorID.MIDDLE );
            float sensor_r = robot.GetSensorReading( (int)SensorID.RIGHT );

            current_angle = robot.GetAngle();
            

            //Calculate angle from current position to the end
            float sx = gx - rx;
            float sy = gy - ry;
            float a = (float)Math.Atan2(sy, sx);

            float d2 = (float)Math.Sqrt((rx - gx) * (rx - gx) + (ry - gy) * (ry - gy));

            //AutobotRunner.GetAutobotRunner().WriteLog("ROBOT=[" + rx + "," + ry + "] " + "GOAL=[" + gx + "," + gy + "] ");
            //AutobotRunner.GetAutobotRunner().WriteLog("angle="+current_angle+", a2goal="+a);

            float delta = 0.01f;

            float left_w = 0;
            float right_w = 0;

            if (Math.Abs(current_angle - a) > delta)
            {
                if (current_angle > a)
                {
                    left_w = 1.0f;
                }
                else
                {
                    right_w = 1.0f;
                }
            }

            if (d2 > 10.0)
            {
                // 1 0 0
                if (sensor_m < 0.99 && sensor_l > 0.99 && sensor_r > 0.99)
                {
                    left_w = 1.0f;
                    right_w = 0.0f;
                }

                // 1 1 0
                if (sensor_m < 0.99 && sensor_l < 0.99 && sensor_r > 0.99)
                {
                    left_w = 0.0f;
                    right_w = 1.0f;
                }

                // 1 0 1
                if (sensor_m < 0.99 && sensor_l > 0.99 && sensor_r < 0.99)
                {
                    left_w = 1.0f;
                    right_w = 0.0f;
                }

                // 0 1 0
                if (sensor_m > 0.99 && sensor_l < 0.99 && sensor_r > 0.99)
                {
                    left_w = 0.0f;
                    right_w = 1.0f;
                }

                // 0 0 1
                if (sensor_m > 0.99 && sensor_l > 0.99 && sensor_r < 0.99)
                {
                    left_w = 1.0f;
                    right_w = 0.0f;
                }

                // 1 1 1
                if (sensor_m < 0.99 && sensor_l < 0.99 && sensor_r < 0.99)
                {
                    if (sensor_m < 0.30 || sensor_l < 0.30 || sensor_r < 0.30) //panic
                    {
                        result.AddCommand(CreateSetDriveMotorCommand((int)DriveMotorParams.STOP, 0));
                        result.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 0));
                    }

                    if (sensor_m < 0.30 || sensor_l < 0.30 || sensor_r < 0.30) //critical
                    {
                        if (sensor_l > sensor_m && sensor_l > sensor_r)
                        {
                            //lefty
                            left_w = 1.0f;
                            right_w = 0.0f;
                        }
                        if (sensor_r > sensor_m && sensor_r > sensor_l)
                        {
                            //righty
                            left_w = 0.0f;
                            right_w = 1.0f;
                        }
                    }
                }
            }

            /*
            if (state == 0)
            {
                if ((sensor_l + sensor_m + sensor_r) < 2.9) //if a sensor is on
                {
                    if ((left_w < right_w)) //left
                    {
                        state = 1;
                    }
                    else //right
                    {
                        state = 2;
                    }
                }
            }

            if (state == 1) //wall follow left
            {
                if ((sensor_l + sensor_m + sensor_r) < 2.9) //if a sensor is on
                {
                    if (Math.Abs(sensor_l - 0.5) > delta)
                    {
                        if (sensor_l > 0.9)
                        {
                            state = 0;
                        }
                        
                        if (sensor_l < 0.6)
                        {
                            //too much, turn AWAAAAY
                            left_w = 1.0f;
                            right_w = 0.0f;
                        }
                    }
                }
                else
                {
                    state = 0; //revert to superman algorithm
                }
            }

            if (state == 2) //wall follow right
            {
                if ((sensor_l + sensor_m + sensor_r) < 2.9) //if a sensor is on
                {
                    if (Math.Abs(sensor_l - 0.5) > delta)
                    {
                        if (sensor_l > 0.9)
                        {
                            state = 0;
                        }

                        if (sensor_l < 0.6)
                        {
                            //too much, turn AWAAAAY
                            left_w = 0.0f;
                            right_w = 1.0f;
                        }
                    }
                }
                else
                {
                    state = 0; //revert to superman algorithm
                }
            }
             * */
            
            if (left_w > right_w)
            {
                result.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 0));
                result.AddCommand(CreateTurnLeftCommand(10, 0));
                result.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 10));
            }

            if (right_w > left_w)
            {
                result.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 0));
                result.AddCommand(CreateTurnRightCommand(10, 0));
                result.AddCommand(CreateSetSteerMotorCommand((int)SteeringMotorParams.STOP, 10));
            }

            

            return result;

        }
    }
}
